"""  
需求：
流程：
    1.导包；
    2.初始化ROS2客户端；
    3.自定义节点类；
                  
    4.调用spain函数，并传入节点对象；
    5.资源释放。


"""
#1.导包；
import rclpy
import math
from rclpy.node import Node
from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster
import tf_transformations
from geometry_msgs.msg import TransformStamped
#3.自定义节点类；
class StaticTFBroadcaster(Node):
    def __init__(self):
        super().__init__("statci_tf_broadcaster_node")
        self.static_tf_broadcaster=StaticTransformBroadcaster(self)     
        self.publish_static_tf()
    def publish_static_tf(self):
        ts=TransformStamped()
        ts.header.stamp=self.get_clock().now().to_msg()
        ts.header.frame_id="base_link"
        ts.child_frame_id="camera_link"   
        ts.transform.translation.x=0.5
        ts.transform.translation.y=0.3
        ts.transform.translation.z=0.6

        qtn=tf_transformations.quaternion_from_euler(
        math.radians(180),0,0)
        ts.transform.rotation.x=qtn[0]
        ts.transform.rotation.y=qtn[1]
        ts.transform.rotation.z=qtn[2]
        ts.transform.rotation.w=qtn[3]

        self.static_tf_broadcaster.sendTransform(ts)
        self.get_logger().info(f"发布TF:{ts}")
def main():
    #2.初始化ROS2客户端；
    rclpy.init()
    #4.调用spain函数，并传入节点对象；
    rclpy.spin(StaticTFBroadcaster())
    #5.资源释放。
    rclpy.shutdown()

if __name__=='__main__':
    main() 